#include <Wire.h>
#include "APM_SRF.h"

#define srfAddress 0x70                           // Address of the SRF08
#define cmdByte 0x00                              // Command byte
#define rangeByte 0x02                            // Byte for start of ranging data


APM_SRF::APM_SRF()
{
	_range = 0;
}

void APM_SRF::init(int initialise_wire_lib)
{
if( initialise_wire_lib != 0 )
      Wire.begin();
}

 int APM_SRF::getRange()
{
  int timeout = 100;

  Wire.beginTransmission(srfAddress);             // start communicating with SRFmodule
  Wire.send(rangeByte);                           // Call the register for start of ranging data
  Wire.endTransmission();
  
  Wire.requestFrom(srfAddress, 2);                // Request 2 bytes from SRF module
  
  while(Wire.available() < 2 && timeout-- > 0);                    // Wait for data to arrive

  _highByte = Wire.receive();                      // Get high byte
  _lowByte = Wire.receive();                       // Get low byte
  _range = (_highByte << 8) + _lowByte;              // Put them together

  return _range;
}

void APM_SRF::fireSonar()
{
	_range = 0;
  Wire.beginTransmission(srfAddress);             // Start communticating with SRF08
  Wire.send(cmdByte);                             // Send Command Byte
  Wire.send(0x51);                                // Send 0x51 to start a ranging
  Wire.endTransmission();
}